#!/usr/bin/env python
from math import sqrt, hypot

import pygame
from pygame import Rect

from lib.geometry import pointOnLine, lineIntersection
from lib.geometry import bezier, lineCurveIntersection, linesCollide
from main import main

class World:
    """The physics' engine 'container' object"""
    def __init__(self, gravity=0.0, conservation=0.0, groundFriction=0.0):
        self.gravity = gravity
        self.conservation = conservation
        self.groundFriction = groundFriction
        
        #CONFIG
        config = main.config
        self.collideAgainstStaticFlag = config.collideAgainstStaticFlag
        self.collideAgainstDynamicFlag = config.collideAgainstDynamicFlag
        
        #DYNAMIC OBJECTS
        self.masses = []
        self.constraints = []
        
        #STATIC OBJECTS FOR COLLISION PURPOSES
        self.points = []
        self.lines = []
        self.curves = []
    
    def update(self):
        for m in self.masses:
            m.verlet(self.conservation)
            m.yPrev -= self.gravity
            
        
        for k in self.constraints:
            k.calculateFix()
        for k in self.constraints:
            k.applyFix()
        
        #Collisions!
        if self.collideAgainstStaticFlag:
            for m in self.masses:
                if m.x != m.xPrev and m.y != m.yPrev:
                    for l in self.lines:
                        if l.AABB.collidepoint(m.x, m.y):
                            if linesCollide(((m.x, m.y), (m.xPrev, m.yPrev)),
                                            ((l.p1.x, l.p1.y), (l.p2.x, l.p2.y))):
                                    projectOnStatic(m, l)
                                    m.conservation(self.groundFriction)
                    for c in self.curves:
                        if c.AABB.collidepoint(m.x, m.y):
                            intersection = lineCurveIntersection(((m.x, m.y),(m.xPrev, m.yPrev)),
                                                                 ((c.p1.x, c.p1.y),
                                                                  (c.p2.x, c.p2.y),
                                                                  (c.p3.x, c.p3.y)))
                            if intersection:
                                projectOnCurve(m, c)
                                m.conservation(self.groundFriction)
            
            if self.collideAgainstDynamicFlag:
                for k in self.constraints:
                    k.AABB.centerx = (k.mass1.x + k.mass2.x) / 2.0
                    k.AABB.centery = (k.mass1.y + k.mass2.y) / 2.0
                
                for k in self.constraints:
                    for m in self.masses:
                        if m != k.mass1 and m != k.mass2:
                            if k.AABB.collidepoint(m.x, m.y):
                                intersection = lineIntersection(((m.x, m.y), (m.xPrev, m.yPrev)),
                                                                ((k.mass1.x, k.mass1.y),
                                                                 (k.mass2.x, k.mass2.y)))
                                if intersection: #Intersection could be calculated
                                    if intersection[2]: #The mass is colliding against the constraint
                                        projectOnDynamic(m, k, intersection)
                                        m.conservation(self.groundFriction)
    
    @property
    def elements(self):
        return self.masses + self.constraints# + self.points + self.lines

class Fluid:
    def __init__(self, length, stiffness):
        self.stiffness = stiffness
        
        self.masses = []
        
    def update(self):
        for i in range(len(self.masses)):
            mass1 = self.masses[i]
            for j in range(i + 1, len(self.masses)):
                mass2 = self.masses[j]
                
                distX = mass2.x - mass1.x
                distY = mass2.y - mass1.y
                
                radii = mass1.weight * 3.0 + mass2.weight * 3.0
                squareRadii = radii ** 2
                
                if -radii < distX < radii and -radii < distY < radii:
                    squareDistance = distX**2 + distY**2
                    if squareDistance < squareRadii:
                        err = (squareDistance - squareRadii) / squareRadii
                        
                        mod = err * self.stiffness
                            
                        mass1.x += distX * mod
                        mass1.y += distY * mod
                        mass2.x -= distX * mod
                        mass2.y -= distY * mod
        
        def getMassCenter(self):
            totalWeight = 0
            cx = 0
            cy = 0
            
            for mass in self.masses:
                totalWeight += mass.weight
                cx += mass.x * mass.weight
                cy += mass.y * mass.weight
            
            cx /= totalWeight
            cy /= totalWeight
            
            return cx, cy

class HomogenousFluid:
    """A faster fluid class, it assumes that particles are equal in size"""
    def __init__(self, length, stiffness):
        self.length = length
        self.squareLength = length**2
        self.stiffness = stiffness
        
        self.masses = []
    
    def update(self):
        for i in range(len(self.masses)):
            mass1 = self.masses[i]
            for j in range(i + 1, len(self.masses)):
                mass2 = self.masses[j]
                
                distX = mass2.x - mass1.x
                distY = mass2.y - mass1.y
                
                if -self.length < distX < self.length and -self.length < distY < self.length:
                    squareDistance = distX**2 + distY**2
                    if squareDistance < self.squareLength:
                        err = (squareDistance - self.squareLength) / self.squareLength
                        
                        mod = err * self.stiffness
                            
                        mass1.xPrev -= distX * mod
                        mass1.yPrev -= distY * mod
                        mass2.xPrev += distX * mod
                        mass2.yPrev += distY * mod

"""Dynamic elements"""
class Point:
    def __init__(self, x, y):
        self.x = x
        self.y = y

class Mass:
    def __init__(self, x, y, weight=1.0):
        self.x = x
        self.y = y
        self.xPrev = x
        self.yPrev = y
        self.weight = weight
    
    def verlet(self, conservation=1.0):
        """Makes the masses keep their movement"""
        self.x, self.xPrev = self.x * (2 - conservation) - self.xPrev * (1 - conservation), self.x
        self.y, self.yPrev = self.y * (2 - conservation) - self.yPrev * (1 - conservation), self.y

class StaticMass(Mass):
    def __init__(self, x, y, weight):
        Mass.__init__(self, x, y, weight)
        self.fixedx = x
        self.fixedy = y
    
    def verlet(self, conservation=1.0):
        """False verlet integration. Keeps the mass still."""
        self.x = self.fixedx
        self.y = self.fixedy

class Constraint:
    def __init__(self, mass1, mass2, length=None, stiffness=1):
        self.mass1 = mass1
        self.mass2 = mass2
        self.stiffness = stiffness
        if length:
            self.length = length
        else:
            self.length = hypot(mass1.x - mass2.x, mass1.y - mass2.y)
        self.squareLength = self.length**2
        
        self.AABB = Rect(0, 0, self.length, self.length)
        self.AABB.center = (mass1.x + mass2.x) / 2, (mass1.y + mass2.y) / 2
        
        #Optimizations
        massTot = self.mass1.weight + self.mass2.weight
        w1 = self.mass2.weight / massTot #Weight multiplier for the first mass
        w2 = self.mass1.weight / massTot
        self.mod1 = self.stiffness * w1 / 4.0
        self.mod2 = self.stiffness * w2 / 4.0
    
    def calculateFix(self):
        """Finds out how much to move masses to achieve correct distance"""
        dx = self.mass2.x - self.mass1.x
        dy = self.mass2.y - self.mass1.y
        squareDistance = dx ** 2 + dy ** 2
        
        error = (squareDistance - self.squareLength) / self.squareLength
        if error > 1.0:
            error = 1.0 #Prevents resonance in case of high conservation
        
        mod1 = error * self.mod1
        mod2 = error * self.mod2
        
        self.x1Fix = dx * mod1
        self.y1Fix = dy * mod1
        self.x2Fix = dx * mod2
        self.y2Fix = dy * mod2
    
    def applyFix(self):
        """Sets the constraint's masses at the right distance"""
        self.mass1.x += self.x1Fix
        self.mass1.y += self.y1Fix
        self.mass2.x -= self.x2Fix
        self.mass2.y -= self.y2Fix
    
    def refresh(self):
        """Takes care of some derived values, like the mass movement multiplier,
        in case their origin(mass weight) is changed"""
        massTot = self.mass1.weight + self.mass2.weight
        w1 = self.mass2.weight / massTot #Weight multiplier for the first mass
        w2 = self.mass1.weight / massTot
        self.mod1 = self.stiffness * w1 / 4.0
        self.mod2 = self.stiffness * w2 / 4.0
        
        self.squareLength = self.length ** 2
        
        self.AABB.size = self.length, self.length
    
    def setLength(self, length):
        self.length = length
        self.squareLength = length ** 2



"""Static elements"""
class Line:
    def __init__(self, point1, point2):
        self.p1 = point1
        self.p2 = point2
        
        self.length = hypot(point2.x - point1.x, point2.y - point1.y)
        
        #Bounding rect
        left = min(point1.x, point2.x)
        top = min(point1.y, point2.y)
        right = max(point1.x, point2.x)
        bottom = max(point1.y, point2.y)
        
        self.AABB = Rect((left, top), (right - left, bottom - top))
    
    def extend(self, amount=0.01):
        """Extends the line by its length * amount"""
        dx = (self.p2.x - self.p1.x) * amount / 2.0
        dy = (self.p2.y - self.p1.y) * amount / 2.0
        
        self.p1.x -= dx
        self.p1.y -= dy
        self.p2.x += dx
        self.p2.y += dy

class Curve:
    def __init__(self, point1, point2, point3):
        self.p1 = point1
        self.p2 = point2
        self.p3 = point3
        
        self.A = point1.x, point1.y
        self.B = point2.x, point2.y
        self.C = point3.x, point3.y
        
        self.length = self.getLength()
        
        left = min(point1.x, point2.x, point3.x)
        top = min(point1.y, point2.y, point3.y)
        right = max(point1.x, point2.x, point3.x)
        bottom = max(point1.y, point2.y, point3.y)
        
        self.AABB = Rect((left, top), (right - left, bottom - top))
    
    def getLength(self, segments=32.0):
        """The length of a curve and the other variables calculated herein
        are extremely useful to calculate the projection of points on this
        curve as precisely and efficiently as possible.
        """
        self.length = 0.0
        
        maxLength = 0.0
        minLength = 255.0 #Won't ever be this high, really.
        
        p1 = self.p1.x, self.p1.y
        for i in xrange(1, int(segments)):
            p2 = bezier((self.p1.x, self.p1.y),
                        (self.p2.x, self.p2.y),
                        (self.p3.x, self.p3.y), segments / float(i))
            
            segmentLength = hypot(p1[0] - p2[0], p1[1] - p2[1])
            
            maxLength = max(maxLength, segmentLength)
            minLength = min(minLength, segmentLength)
            
            self.length += segmentLength
            
            p1 = p2 #Prepare for the next iteration
        
        self.maxVariation = maxLength / minLength
        
        return self.length

"""Collision methods

There are many ways to simulate collision reaction once a collision is
properly detected. I am currently experimenting to see which ones perform the
best, and which ones look correct enough."""
def bounceOnStatic(mass, line, intersection):
    """Fast bouncing fuction devoid of sqrts.
    
    Credits to Lorenzo Porzi for the maths and Java code this is based upon"""
    
    #Calculate mass direction
    v = line.p2.x - line.p1.x, line.p2.y - line.p1.y #Line vector
    mv = mass.x - mass.xPrev, mass.y - mass.yPrev #Mass movement vector
    
    modV2 = v[0] ** 2 + v[1] ** 2
    
    rx = (mv[0] * (v[0]**2 - v[1]**2) + 2*v[0]*v[1]*mv[1]) / modV2
    ry = (mv[1] * (v[1]**2 - v[0]**2) + 2*v[0]*v[1]*mv[0]) / modV2
    
    mass.xPrev, mass.yPrev, spam = intersection
    mass.x, mass.y = mass.xPrev + rx, mass.yPrev + ry

def bounceOnDynamic(mass, line, intersection=False):
    """Fast bouncing fuction devoid of sqrts.
    
    Credits to Lorenzo Porzi for all of this function's code"""
    
    if intersection == False:
        intersection = lineIntersection(((mass.x, mass.y), (mass.xPrev, mass.yPrev)), line)
        intersection.pop()
    
    peso = line.m1.weight + line.m2.weight + mass.weight
    
    #Calculate mass direction
    v = (line.m2.x - line.m1.x, line.m2.y - line.m1.y) #The line vector
    mv = (mass.x - mass.xPrev, mass.y - mass.yPrev) #The mass movement vector
    
    modV2 = v[0]**2 + v[1]**2
    
    rx = (mv[0] * (v[0]**2 - v[1]**2) + 2*v[0]*v[1]*mv[1]) / (modV2 * sqrt(mv[0]**2 + mv[1]**2))
    ry = (mv[1] * (v[1]**2 - v[0]**2) + 2*v[0]*v[1]*mv[0]) / (modV2 * sqrt(mv[0]**2 + mv[1]**2))
    
    quantx = (line.m1.x - line.m1.xPrev) * line.m1.weight + (line.m2.x-line.m2.xPrev) * line.m2.weight + mv[0] * mass.weight
    quanty = (line.m1.y - line.m1.yPrev) * line.m1.weight + (line.m2.y-line.m2.yPrev) * line.m2.weight + mv[1] * mass.weight
    quantM1x = quant[0] / peso * mass.weight
    quantM1y = quant[1] / peso * mass.weight
    quantM2x = quant[0] / peso * line.m1.weight
    quantM2y = quant[1] / peso * line.m1.weight
    quantM3x = quant[0] / peso * line.m2.weight
    quantM3y = quant[1] / peso * line.m2.weight
    
    mass.xPrev, mass.yPrev, spam = intersection
    mass.x, mass.y = mass.xPrev + rx * quantM1x / mass.weight, mass.yPrev + ry * quantM1y / mass.weight
    
    vm1 = (line.m1.x - line.m1.xPrev, line.m1.y - line.m1.yPrev)
    vm2 = (line.m2.x - line.m2.xPrev, line.m2.y - line.m2.yPrev)
    
    line.m1.xPrev = line.m1.x
    line.m1.yPrev = line.m1.y
    line.m2.xPrev = line.m2.x
    line.m2.yPrev = line.m2.y
    
    line.m1.x = line.m1.x + vm1[0] + quantM2x / line.m1.weight
    line.m1.y = line.m1.y + vm1[1] + quantM2y / line.m1.weight
    line.m2.x = line.m2.x + vm2[0] + quantM3x / line.m2.weight
    line.m2.y = line.m2.y + vm2[1] + quantM3y / line.m2.weight

def projectOnStatic(mass, line, intersection=None):   
    mass.x, mass.y = pointOnLine(mass.x, mass.y, line.p1.x, line.p1.y,
                                                 line.p2.x, line.p2.y)

def projectOnCurve(mass, curve, intersection=None):
    lowerLimit = 0.0
    upperLimit = 1.0
    x1, y1 = bezier(curve.A, curve.B, curve.C, lowerLimit)
    x2, y2 = bezier(curve.A, curve.B, curve.C, upperLimit)
    for i in range(6):
        if (mass.x - x1) ** 2 + (mass.y - y1) ** 2 < \
           (mass.x - x2) ** 2 + (mass.y - y2) ** 2:
            upperLimit = (upperLimit + lowerLimit) / 2.0
            x2, y2 = bezier(curve.A, curve.B, curve.C, upperLimit)
        else:
            lowerLimit = (upperLimit + lowerLimit) / 2.0
            x1, y1 = bezier(curve.A, curve.B, curve.C, lowerLimit)
    mass.x = (x1 + x2) / 2.0
    mass.y = (y1 + y2) / 2.0

def projectOnDynamic(mass, k, intersection=None):
    """Doesn't take into account weight"""
    oldx = mass.x
    oldy = mass.y
    mass.x, mass.y, d = pointOnLine(mass.x, mass.y, k.mass1.x, k.mass1.y,
                                                    k.mass2.x, k.mass2.y)
    xMov = mass.x - oldx
    yMov = mass.y - oldy
    k.mass1.x -= xMov * (1 - d)
    k.mass1.y -= yMov * (1 - d)
    k.mass2.x -= xMov * d
    k.mass2.y -= yMov * d
